#include "funcio.h"
#include <map>
#include "robfunc.h"

using funcio::FuncInItf;
using funcio::FuncInType;

static std::shared_ptr<funcio::RobPowerOn> _rob_power_on;
static std::shared_ptr<funcio::RobPowerOff> _rob_power_off;

int funcio::init_funcio()
{
    _rob_power_off = std::make_shared<funcio::RobPowerOff>();
    _rob_power_on = std::make_shared<funcio::RobPowerOn>();
    return 0;
}
std::shared_ptr<funcio::FuncInItf> funcio::func_in(FuncInType type)
{
    switch (type)
    {
    case FuncInType::RobPowerOn:
        return _rob_power_on;
    case FuncInType::RobPowerOff:
        return _rob_power_off;
    // case FuncInType::RobEnable:
    //     break;
    // case FuncInType::RobDisable:
    //     break;
    // case FuncInType::RobBackInitialPos:
    //     break;
    // case FuncInType::RobDragOn:
    //     break;
    // case FuncInType::RobDragOff:
    //     break;
    // case FuncInType::RobPercentageModeLv1:
    //     break;
    // case FuncInType::RobPercentageModeLv2:
    //     break;
    // case FuncInType::RobStopMode:
    //     break;
    // case FuncInType::ErrorReset:
    //     break;
    // case FuncInType::TaskRun:
    //     break;
    // case FuncInType::TaskStop:
    //     break;
    // case FuncInType::TaskPause:
    //     break;
    // case FuncInType::TaskResume:
    //     break;
    default:
        break;
    }
    return nullptr;
}

void funcio::check_func_io(void)
{
    // TODO:迁移到独立的功能IO模块中

    // static int autoenablework = 0;
    // // static int proautorun = 0;

    // static int tio_prog_resume_req = 0;
    // static int tio_prog_pause_req = 0;
    // static int clsn_prog_pause_req = 0;
    // hal_bit_t temp_di_work;  // used to stage the di state
    // IOTypeIndex func_di, func_do;
    // static int funcio_in_erropmode = 0;
    // static int erropmode_flag = 0;  // 错误的手自动模式下触发功能di时，此变量对应的bit置1
    // int* erropmode_flag_ptr = &erropmode_flag;
    // static int last_tio_di_pause_resume = 0;
    // static int last_scb_di_power = 0;
    // static int last_scb_di_servo = 0;
    // static int last_scb_di_run_stop = 0;
    // static int last_scb_di_pause_resume = 0;
    // static int scb_power_on_req = 0;
    // static int scb_power_off_req = 0;
    // static int scb_servo_on_req = 0;
    // static int scb_servo_off_req = 0;
    // static int scb_prog_run_req = 0;
    // static int scb_prog_stop_req = 0;
    // static int scb_prog_pause_req = 0;
    // static int scb_prog_resume_req = 0;
    // int scb_init_pos_done = 0;
    // static int collision_start_done = 0;
    // if (zucmotStatus->operation_mode == 0 && zucmotStatus->operation_mode_option == 1)
    // {
    //     funcio_in_erropmode = 1;
    // }
    // else
    // {
    //     funcio_in_erropmode = 0;
    // }
    // if (zucmotConfig->cab_type > 1)
    // {
    //     if (zucmotConfig->cab_type == 2)
    //     {
    //         (zucmot_hal_data->sys_shutdown) = get_scb_poweroff_req();
    //     }
    //     else if (zucmotConfig->cab_type == 3)
    //     {
    //         (zucmot_hal_data->sys_shutdown) = get_scb_poweroff_req();
    //     }

    //     BPHandleButton bp_button_event = {};
    //     get_bp_button_event_status(&bp_button_event);

    //     zucmotStatus->scb_stick_locked = bp_button_event.bp_lock_on_off;
    //     // pre-process of scb function digital input
    //     if (!last_scb_di_run_stop && bp_button_event.bp_run_stop_on_off)
    //     {
    //         if ((zucmot_hal_data->program_stat_running) || (zucmot_hal_data->program_stat_paused))
    //         {
    //             scb_prog_stop_req = 1;  // #TODO 需要传到task
    //         }
    //         else
    //         {
    //             scb_prog_run_req = 1;  // #TODO 需要传到task
    //         }
    //     }
    //     else if (last_scb_di_run_stop && !bp_button_event.bp_run_stop_on_off)
    //     {
    //         scb_prog_stop_req = 0;  // #TODO 需要传到task
    //         scb_prog_run_req = 0;   // #TODO 需要传到task
    //     }
    //     if (!last_scb_di_pause_resume && bp_button_event.bp_pause_resume_on_off)
    //     {
    //         if ((zucmot_hal_data->program_stat_paused))
    //         {
    //             scb_prog_resume_req = 1;  // #TODO 需要传到task
    //         }
    //         else if ((zucmot_hal_data->program_stat_running))
    //         {
    //             scb_prog_pause_req = 1;  // #TODO 需要传到task
    //         }
    //     }
    //     else if (last_scb_di_pause_resume && !bp_button_event.bp_pause_resume_on_off)
    //     {
    //         scb_prog_resume_req = 0;
    //         scb_prog_pause_req = 0;
    //     }
    //     if (!last_scb_di_power && bp_button_event.bp_power_robot_on_off && zucmotStatus->scb_stick_locked)
    //     {
    //         if (zucmotStatus->powered_on)
    //         {
    //             scb_power_off_req = 1;
    //             scb_power_robot(false);
    //             (zucmot_hal_data->zuc_enable_cmd) = 0;  // 当前不向外传，所以在这里把这个置位
    //         }
    //         else
    //         {
    //             scb_power_on_req = 1;
    //             scb_power_robot(true);
    //             (zucmot_hal_data->zuc_enable_cmd) = 1;  // 当前不向外传，所以在这里把这个置位
    //         }
    //     }
    //     else if (last_scb_di_power && !bp_button_event.bp_power_robot_on_off)
    //     {
    //         scb_power_off_req = 0;
    //         scb_power_on_req = 0;
    //     }
    //     if (!last_scb_di_servo && bp_button_event.bp_en_robot_on_off && zucmotStatus->scb_stick_locked)
    //     {
    //         if (GET_MOTION_ENABLE_FLAG())
    //         {
    //             scb_servo_off_req = 1;
    //             zucmotDebug->enabling = 0;
    //             zucmotDebug->teleoperating = 0;
    //             zucmotDebug->teleoperating_tool = 0;
    //             zucmotDebug->dragging = 0;
    //             zucmotDebug->servomoveoperating = 0;
    //             zucmotDebug->admitting = 0;
    //             if (zucmotStatus->category_one_stop_program_pause == 0)
    //             {
    //                 zucmotDebug->coordinating = 0;
    //             }
    //         }
    //         else
    //         {
    //             zucmotDebug->enabling = 1;
    //             zucmotDebug->teleoperating = 0;
    //             zucmotDebug->teleoperating_tool = 0;
    //             zucmotDebug->dragging = 0;
    //             zucmotDebug->servomoveoperating = 0;
    //             zucmotDebug->admitting = 0;
    //             if (zucmotStatus->category_one_stop_program_pause == 0)
    //             {
    //                 zucmotDebug->coordinating = 0;
    //             }
    //             scb_servo_on_req = 1;
    //         }
    //     }
    //     else if (last_scb_di_servo && !bp_button_event.bp_en_robot_on_off)
    //     {
    //         scb_servo_off_req = 0;
    //         scb_servo_on_req = 0;
    //     }

    //     for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    //     {
    //         if (collisionStateUpdate(joint_num) == true)
    //         {
    //             collision_start_done = (int)servoStatus[joint_num].collisionState;
    //             break;
    //         }
    //     }
    // }
    // // tio_button
    // // pre-process of tio function di
    // if (zucmotStatus->progButtonOpt == ON)
    // {
    //     if (!last_tio_di_pause_resume && zucmotStatus->tio_status.UIKey_pause_resume)
    //     {
    //         if ((zucmot_hal_data->program_stat_paused))
    //         {
    //             tio_prog_resume_req = 1;
    //         }
    //         else if ((zucmot_hal_data->program_stat_running))
    //         {
    //             tio_prog_pause_req = 1;
    //         }
    //     }
    // }

    // if (last_tio_di_pause_resume && !zucmotStatus->tio_status.UIKey_pause_resume)
    // {
    //     if (zucmotStatus->progButtonOpt == OFF && zucmotStatus->motion_state == ZUCMOT_MOTION_COORD)
    //     {
    //         reportInfo(CANNOT_PAUSE_RESUME_PROG_WHEN_TURN_OFF_PROG_BUTTON, "can not pause or resume program when turn off the button");
    //     }
    //     else
    //     {
    //         tio_prog_resume_req = 0;
    //         tio_prog_pause_req = 0;
    //     }
    // }

    // // function DI program stop request handling
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.task_stop_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (zucmotConfig->cab_type > 1 && zucmotStatus->scb_stick_locked)
    // {
    //     temp_di_work = temp_di_work || scb_prog_stop_req;
    // }
    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, PROG_STOP_BIT) == 0 && (zucmot_hal_data->program_stop_req) == 0)
    // {
    //     if (funcio_in_erropmode)
    //     {
    //         set_bit(erropmode_flag_ptr, PROG_STOP_BIT);
    //         reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(program stop) cannot be triggerred in manual mode");
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_PROGSTOP_TRIGED, "function DI(program stop) triggered");
    //     }
    // }
    // else if (temp_di_work == 0 || zucmotStatus->operation_mode == 1)
    // {
    //     clear_bit(erropmode_flag_ptr, PROG_STOP_BIT);
    // }
    // (zucmot_hal_data->program_stop_req) = temp_di_work && !funcio_in_erropmode && check_bit(erropmode_flag_ptr, PROG_STOP_BIT) == 0;
    // zucmotStatus->func_io_req.program_stop_req = (zucmot_hal_data->program_stop_req);

    // // function DI program run request
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.task_run_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (zucmotConfig->cab_type > 1 && zucmotStatus->scb_stick_locked)
    // {
    //     temp_di_work = temp_di_work || scb_prog_run_req;
    // }
    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, PROG_RUN_BIT) == 0 && (zucmot_hal_data->program_run_req) == 0)
    // {
    //     if (funcio_in_erropmode)
    //     {
    //         set_bit(erropmode_flag_ptr, PROG_RUN_BIT);
    //         reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(program run) cannot be triggerred in manual mode");
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_PROGRUN_TRIGED, "function DI(program run) triggered");
    //     }
    // }
    // else if (temp_di_work == 0)
    // {
    //     clear_bit(erropmode_flag_ptr, PROG_RUN_BIT);
    // }
    // (zucmot_hal_data->program_run_req) =
    //     (zucmot_hal_data->program_run_req) ? temp_di_work : (temp_di_work && !funcio_in_erropmode && check_bit(erropmode_flag_ptr, PROG_RUN_BIT) == 0);

    // // function DI program pause request handling
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.task_pause_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // temp_di_work = temp_di_work || tio_prog_pause_req;
    // if (zucmotConfig->cab_type > 1 && zucmotStatus->scb_stick_locked)
    // {
    //     temp_di_work = temp_di_work || scb_prog_pause_req;
    // }
    // if (zucmotStatus->protective_stop && zucmotConfig->safetyCfg.clsnOpt.reactionType)
    // {
    //     zucmot_joint_t* joint;
    //     for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    //     {
    //         joint = &joints[joint_num];
    //         joint->free_tp.enable = 0;
    //     }
    //     // if(!GET_MOTION_INPOS_FLAG()){
    //     // 	servoClearCollision();
    //     // }
    //     if ((zucmot_hal_data->program_stat_running))
    //     {
    //         clsn_prog_pause_req = 1;
    //     }
    //     else if ((zucmot_hal_data->program_stat_idle))
    //     {  // jog_movl
    //    //         tpManager_tpabort();
    //     }
    // }
    // else
    // {
    //     clsn_prog_pause_req = 0;
    // }

    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, PROG_PAUSE_BIT) == 0 && (zucmot_hal_data->program_pause_req) == 0)
    // {
    //     if (funcio_in_erropmode)
    //     {
    //         set_bit(erropmode_flag_ptr, PROG_PAUSE_BIT);
    //         reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(program pause) cannot be triggerred in manual mode");
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_PROGPAUSE_TRIGED, "function DI(program pause) triggered");
    //     }
    // }
    // else if (temp_di_work == 0 || zucmotStatus->operation_mode == 1)
    // {
    //     clear_bit(erropmode_flag_ptr, PROG_PAUSE_BIT);
    // }
    // // temp_di_work = temp_di_work || (zucmotConfig->cat1StopEffect.program_pause && zucmotStatus->category_one_stop);
    // // temp_di_work = temp_di_work || zucmotStatus->protective_stop;
    // // zucmotStatus->func_io_req.program_pause_req = temp_di_work;

    // (zucmot_hal_data->program_pause_req) = (temp_di_work && !funcio_in_erropmode && check_bit(erropmode_flag_ptr, PROG_PAUSE_BIT) == 0) ||
    //                                        (zucmotConfig->cat1StopEffect.program_pause && zucmotStatus->category_one_stop);
    // zucmotStatus->func_io_req.program_pause_req = (zucmot_hal_data->program_pause_req);

    // //collision program pause request handling
    // (zucmot_hal_data->collision_program_pause_req) = clsn_prog_pause_req ? 1 : 0;

    // // function DI program resume request handling
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.task_resume_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // temp_di_work = temp_di_work || tio_prog_resume_req;
    // if (zucmotConfig->cab_type > 1 && zucmotStatus->scb_stick_locked)
    // {
    //     temp_di_work = temp_di_work || scb_prog_resume_req;
    // }

    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, PROG_RESUME_BIT) == 0 && (zucmot_hal_data->program_resume_req) == 0)
    // {
    //     if (funcio_in_erropmode)
    //     {
    //         set_bit(erropmode_flag_ptr, PROG_RESUME_BIT);
    //         reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(program resume) cannot be triggerred in manual mode");
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_PROGRESUME_TRIGED, "function DI(program resume) triggered");
    //     }
    // }
    // else if (temp_di_work == 0)
    // {
    //     clear_bit(erropmode_flag_ptr, PROG_RESUME_BIT);
    // }
    // (zucmot_hal_data->program_resume_req) =
    //     (zucmot_hal_data->program_resume_req) ? temp_di_work : (temp_di_work && !funcio_in_erropmode && check_bit(erropmode_flag_ptr, PROG_RESUME_BIT) == 0);

    // // function DI power off request handling
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.power_off_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (zucmotConfig->cab_type > 1 && zucmotStatus->scb_stick_locked)
    // {
    //     temp_di_work = temp_di_work || scb_power_off_req;
    // }
    // if (temp_di_work == 1 && (zucmot_hal_data->power_off_req) == 0)
    // {
    //     reportInfo(INFO_FDI_POWEROFF_TRIGED, "function DI(power off) triggered");
    // }
    // (zucmot_hal_data->power_off_req) = temp_di_work;
    // zucmotStatus->func_io_req.power_off_req = temp_di_work;

    // if (GET_MOTION_INPOS_FLAG())
    // {
    //     if (GET_MOTION_ENABLE_FLAG() && (zucmotStatus->autoprogram == 1))
    //     {
    //         if (enable_last_stat < enable_complete)
    //         {
    //             (zucmot_hal_data->pro_autorun_req) = 1;
    //         }
    //     }
    //     else
    //     {
    //         (zucmot_hal_data->pro_autorun_req) = 0;
    //     }

    //     if (enable_complete == 0)
    //     {
    //         enable_last_stat = enable_complete;
    //     }
    //     else
    //     {  // reset the autorun_req after enabling
    //         if (enable_last_stat >= 20)
    //         {
    //             (zucmot_hal_data->pro_autorun_req) = 0;
    //         }
    //         else
    //         {
    //             ++enable_last_stat;
    //         }
    //     }
    // }

    // //auto enbaled handling
    // if (GET_MOTION_ENABLE_FLAG())
    // {
    //     autoenablework = 1;
    // }
    // else if (!(zucmot_hal_data->zuc_enable_cmd))
    // {
    //     autoenablework = 0;
    // }

    // if ((zucmot_hal_data->robot_powered_on) && (zucmotStatus->onekeyenable == 1) && !autoenablework)
    // {
    //     (zucmot_hal_data->delay_enable_req) = 1;
    // }
    // else
    // {
    //     (zucmot_hal_data->delay_enable_req) = 0;
    // }

    // // power on request handling
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.power_on_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (zucmotConfig->cab_type > 1 && zucmotStatus->scb_stick_locked)
    // {
    //     temp_di_work = temp_di_work || scb_power_on_req;
    // }
    // if (temp_di_work == 1 && (zucmot_hal_data->power_on_req) == 0)
    // {
    //     reportInfo(INFO_FDI_POWERON_TRIGED, "function DI(power on) triggered");
    // }
    // (zucmot_hal_data->power_on_req) = temp_di_work;

    // // function DI(servo enable) request handling
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.servo_enable_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (temp_di_work == 1 && (zucmot_hal_data->servo_enable_req) == 0)
    // {
    //     temp_di_work = temp_di_work || scb_servo_on_req;
    // }
    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, SERVO_ON_BIT) == 0 && (zucmot_hal_data->servo_enable_req) == 0)
    // {
    //     if (funcio_in_erropmode)
    //     {
    //         set_bit(erropmode_flag_ptr, SERVO_ON_BIT);
    //         reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(servo enable) cannot be triggerred in manual mode");
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_SERVOON_TRIGED, "function DI(servo enable) triggered");
    //     }
    // }
    // else if (temp_di_work == 0)
    // {
    //     clear_bit(erropmode_flag_ptr, SERVO_ON_BIT);
    // }
    // (zucmot_hal_data->servo_enable_req) =
    //     (zucmot_hal_data->servo_enable_req) ? temp_di_work : (temp_di_work && !funcio_in_erropmode && check_bit(erropmode_flag_ptr, SERVO_ON_BIT) == 0);

    // // bp stick DI servo off request handling
    // if (scb_servo_on_req == 1 && (zucmot_hal_data->bp_servo_enable_req) == 0)
    // {
    //     reportInfo(INFO_FDI_SERVOON_TRIGED, "bp stick DI(servo enable) triggered");
    // }
    // (zucmot_hal_data->bp_servo_enable_req) = scb_servo_on_req;

    // // servo off request handling
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.servo_disable_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (zucmotConfig->cab_type > 1 && zucmotStatus->scb_stick_locked)
    // {
    //     temp_di_work = temp_di_work || scb_servo_off_req;
    // }
    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, SERVO_OFF_BIT) == 0 && (zucmot_hal_data->servo_disable_req) == 0)
    // {
    //     if (funcio_in_erropmode)
    //     {
    //         set_bit(erropmode_flag_ptr, SERVO_OFF_BIT);
    //         reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(servo disable) cannot be triggerred in manual mode");
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_SERVOOFF_TRIGED, "function DI(servo disable) triggered");
    //     }
    // }
    // else if (temp_di_work == 0)
    // {
    //     clear_bit(erropmode_flag_ptr, SERVO_OFF_BIT);
    // }

    // (zucmot_hal_data->servo_disable_req) =
    //     (zucmot_hal_data->servo_disable_req) ? temp_di_work : (temp_di_work && !funcio_in_erropmode && check_bit(erropmode_flag_ptr, SERVO_OFF_BIT) == 0);
    // zucmotStatus->func_io_req.servo_disable_req = (zucmot_hal_data->servo_disable_req);

    // BPHandleButton bp_button_event;
    // get_bp_button_event_status(&bp_button_event);
    // // function DI back to home request handling
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.back_to_inipos_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (zucmotConfig->cab_type > 1 && zucmotStatus->scb_stick_locked)
    // {
    //     temp_di_work = temp_di_work || bp_button_event.bp_return_init_on_off;
    // }
    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, BACK_TO_INIT_POS_BIT) == 0 && (zucmot_hal_data->return_to_initial_position) == 0)
    // {
    //     if (funcio_in_erropmode)
    //     {
    //         set_bit(erropmode_flag_ptr, BACK_TO_INIT_POS_BIT);
    //         reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(to initial position) cannot be triggerred in manual mode");
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_BACKTOINITPOS_TRIGED, "function DI(to initial position) triggered");
    //     }
    // }
    // else if (temp_di_work == 0)
    // {
    //     clear_bit(erropmode_flag_ptr, BACK_TO_INIT_POS_BIT);
    // }
    // (zucmot_hal_data->return_to_initial_position) = (zucmot_hal_data->return_to_initial_position) ?
    //                                                     temp_di_work :
    //                                                     (temp_di_work && !funcio_in_erropmode && check_bit(erropmode_flag_ptr, BACK_TO_INIT_POS_BIT) == 0);

    // // function DI percentage mode level 1 request handling
    // temp_di_work = 1;  // anti-logical
    // func_di = zucmotStatus->funcIOMap.percentage_mode_level1_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (temp_di_work == 0 && (zucmot_hal_data->percentage_mode_level1_req) == 0)
    // {
    //     reportInfo(INFO_FDI_PERCENTAGE_MODE_LEVEL1_TRIGED, "function DI(percentage mode level 1) triggered");
    // }
    // (zucmot_hal_data->percentage_mode_level1_req) = temp_di_work ? 0 : 1;

    // //function DI percentage mode level 2 request handling
    // temp_di_work = 1;  // anti-logical
    // func_di = zucmotStatus->funcIOMap.percentage_mode_level2_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (temp_di_work == 0 && (zucmot_hal_data->percentage_mode_level2_req) == 0)
    // {
    //     reportInfo(INFO_FDI_PERCENTAGE_MODE_LEVEL2_TRIGED, "function DI(percentage mode 2) triggered");
    // }
    // zucmot_hal_data->percentage_mode_level2_req = temp_di_work ? 0 : 1;

    // //function DI protective stop
    // temp_di_work = 1;  // anti-logical
    // func_di = zucmotStatus->funcIOMap.stop_mode_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (temp_di_work == 0 && (zucmot_hal_data->stop_mode_req) == 0)
    // {
    //     reportInfo(INFO_FDI_PROTECTIVE_STOP_TRIGED, "function DI(protective stop) triggered");
    // }
    // zucmot_hal_data->stop_mode_req = temp_di_work ? 0 : 1;

    // //function DI clear error
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.clear_error_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (temp_di_work == 1 && (zucmot_hal_data->clear_error_req) == 0)
    // {
    //     reportInfo(INFO_FDI_CLEARERROR_TRIGED, "function DI(clear error) triggered");
    // }
    // zucmot_hal_data->clear_error_req = temp_di_work;

    // //function DI enable drag mode
    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.drag_mode_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, ENABLE_DRAG_BIT) == 0 && (zucmot_hal_data->drag_mode_entr_req) == 0)
    // {
    //     if (zucmotStatus->operation_mode_option == 1)
    //     {
    //         if (zucmotStatus->operation_mode == 0)
    //         {
    //             reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(reduce stop) cannot be triggerred in manual mode");
    //         }
    //         else
    //         {
    //             reportError(CANNOT_TRIGGER_FUNCTION_IN_AUTOMODE, "function DI(enable drag mode) cannot be triggerred in auto mode");
    //         }
    //         set_bit(erropmode_flag_ptr, ENABLE_DRAG_BIT);
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_DRAGMODEON_TRIGED, "function DI(enable drag mode) triggered");
    //     }
    // }
    // else if (temp_di_work == 0)
    // {
    //     clear_bit(erropmode_flag_ptr, ENABLE_DRAG_BIT);
    // }
    // zucmot_hal_data->drag_mode_entr_req = temp_di_work && check_bit(erropmode_flag_ptr, ENABLE_DRAG_BIT) == 0;

    // temp_di_work = 0;
    // func_di = zucmotStatus->funcIOMap.drag_mode_disable_di;
    // get_func_di_stat(func_di, &temp_di_work);
    // temp_di_work = temp_di_work || three_position_enabling_exit_drag_req;
    // if (temp_di_work == 1 && check_bit(erropmode_flag_ptr, DISABLE_DRAG_BIT) == 0 && (zucmot_hal_data->drag_mode_exit_req) == 0)
    // {
    //     if (zucmotStatus->operation_mode_option == 1)
    //     {
    //         if (zucmotStatus->operation_mode == 0)
    //         {
    //             reportError(CANNOT_TRIGGER_FUNCTION_IN_MANUALMODE, "function DI(disable drag mode) cannot be triggerred in manual mode");
    //         }
    //         else
    //         {
    //             reportError(CANNOT_TRIGGER_FUNCTION_IN_AUTOMODE, "function DI(disable drag mode) cannot be triggerred in auto mode");
    //         }
    //         set_bit(erropmode_flag_ptr, DISABLE_DRAG_BIT);
    //     }
    //     else
    //     {
    //         reportInfo(INFO_FDI_DRAGMODEOFF_TRIGED, "function DI(disable drag mode) triggered");
    //     }
    // }
    // else if (temp_di_work == 0)
    // {
    //     clear_bit(erropmode_flag_ptr, DISABLE_DRAG_BIT);
    // }
    // zucmot_hal_data->drag_mode_exit_req = temp_di_work && check_bit(erropmode_flag_ptr, DISABLE_DRAG_BIT) == 0;

    // /*functional outputs*/
    // for (int i = 0; i < 4; i++)
    // {
    //     func_do = zucmotStatus->funcIOMap.task_idle_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->program_stat_idle);
    //     func_do = zucmotStatus->funcIOMap.task_paused_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->program_stat_paused);
    //     func_do = zucmotStatus->funcIOMap.task_running_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->program_stat_running);
    //     func_do = zucmotStatus->funcIOMap.stat_fault_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->coord_error);
    //     func_do = zucmotStatus->funcIOMap.stat_poweron_do[i];
    //     set_func_do_stat(func_do, zucmotStatus->powered_on);
    //     //  check_poweron_stat(*zucmot_hal_data->robot_poweron, *zucmot_hal_data->robot_enabled));  // 功能IO上电，非阻塞式延时设置机器人上电信号
    //     func_do = zucmotStatus->funcIOMap.stat_enabled_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->robot_enabled);
    //     func_do = zucmotStatus->funcIOMap.stat_motion_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->robot_motion);
    //     func_do = zucmotStatus->funcIOMap.stat_static_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->robot_static);
    //     func_do = zucmotStatus->funcIOMap.stat_boot_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->robot_boot);
    //     func_do = zucmotStatus->funcIOMap.stat_emergency_stop_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->robot_emergency_stop);
    //     func_do = zucmotStatus->funcIOMap.stat_percentage_mode_level2_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->robot_in_percentage_mode_level2);
    //     func_do = zucmotStatus->funcIOMap.stat_sys_protective_stop_do[i];
    //     set_func_do_stat(func_do, zucmot_hal_data->robot_sys_protective_stop);
    //     func_do = zucmotStatus->funcIOMap.stat_initial_pose_do[i];
    //     set_func_do_stat(func_do, (zucmot_hal_data->in_home_position));
    //     //一级缩减状态DO
    //     func_do = zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i];
    //     set_func_do_stat(func_do, (zucmot_hal_data->robot_in_percentage_mode_level1));
    //     //拖拽状态DO
    //     func_do = zucmotStatus->funcIOMap.stat_drag_mode_do[i];
    //     set_func_do_stat(func_do, (zucmot_hal_data->robot_drag_mode));
    //     //碰撞状态DO
    //     func_do = zucmotStatus->funcIOMap.stat_collision_mode_do[i];
    //     set_func_do_stat(func_do, collision_start_done);

    //     //手动模式状态
    //     func_do = zucmotStatus->funcIOMap.stat_manual_mode_do[i];
    //     set_func_do_stat(func_do, !zucmotStatus->operation_mode);

    //     //自动模式状态
    //     func_do = zucmotStatus->funcIOMap.stat_auto_mode_do[i];
    //     set_func_do_stat(func_do, zucmotStatus->operation_mode);
    // }

    // if (zucmotConfig->cab_type > 1)
    // {
    //     /*update the SCB DO status*/
    //     scb_init_pos_done = 1;
    //     for (int i = 0; i < zucmotConfig->numJoints; i++)
    //     {
    //         if (fabs(joints[i].pos_cmd - zucmotConfig->safetyCfg.init_traj_jpos[i]) >= 0.001)
    //         {
    //             scb_init_pos_done = 0;
    //             break;
    //         }
    //     }
    //     set_bp_in_init_pos(scb_init_pos_done);
    //     last_scb_di_power = bp_button_event.bp_power_robot_on_off;
    //     last_scb_di_servo = bp_button_event.bp_en_robot_on_off;
    //     last_scb_di_run_stop = bp_button_event.bp_run_stop_on_off;
    //     last_scb_di_pause_resume = bp_button_event.bp_pause_resume_on_off;
    // }

    // last_tio_di_pause_resume = zucmotStatus->tio_status.UIKey_pause_resume;
}

static void get_func_di_stat(IOTypeIndex func_di, bool* di_stat)
{
    // if (func_di.type == 0 && func_di.index >= 0 && func_di.index < zucmotConfig->cabTotalNumDIO)
    // {  // basic io
    //     *di_stat = zucmotStatus->synch_di[func_di.index];
    // }
    // else if (func_di.type == 1 && func_di.index >= 0 && func_di.index < 2)
    // {  // tool io
    //     *di_stat = (zucmotStatus->tio_status.DI_buffer >> func_di.index) & 0x01;
    // }
    // else if (func_di.type == 2 && func_di.index >= 0 && func_di.index < MAX_EXTIO_DI_NUM)
    // {  //extio
    //     *di_stat = zucmotStatus->extio_stat.extDI[func_di.index];
    // }
    // else if (func_di.type == 4 && func_di.index >= 0 && func_di.index < ZUCMOT_MAX_MODBUS_DIO)
    // {
    //     *di_stat = zucmotStatus->synch_mb_slave_di[func_di.index];
    // }
    // else if (func_di.type == 5 && func_di.index >= 0 && func_di.index < ZUCMOT_MAX_PNDev_DIO)
    // {
    //     *di_stat = zucmotStatus->synch_pn_dev_di[func_di.index];
    // }
    // else if (func_di.type == 6)
    // {
    //     if (zucmotStatus->eip_mode == 1 && func_di.index >= 0 && func_di.index < zucmotStatus->eip_di_num)
    //     {
    //         int byteNum = func_di.index / 8;
    //         func_di.index -= byteNum * 8;
    //         *di_stat = 0x01 & (0xff & (zucmotStatus->synch_eip_in[byteNum] >> func_di.index));
    //     }
    //     else if (zucmotStatus->eip_mode == 0 && func_di.index >= 0 && func_di.index < ZUCMOT_MAX_EIP_DIO)
    //     {
    //         *di_stat = zucmotStatus->synch_eip_adpt_di[func_di.index];
    //     }
    // }
}

static void set_func_do_stat(IOTypeIndex func_do, int do_stat)
{
    // if (func_do.type == 0 && func_do.index >= 0 && func_do.index < zucmotConfig->cabTotalNumDIO)
    // {
    //     if (func_do.index < zucmotConfig->cabUsrPhysNumDIO)
    //     {
    //         if (zucmotConfig->cab_type >= 1)
    //         {
    //             zucmotPhysicIOStruct->physic_do[func_do.index] = do_stat;
    //         }
    //     }
    //     else if (func_do.index < zucmotConfig->cabUsrPhysNumDIO + ZUCMOT_MAX_MODBUS_DIO)
    //     {
    //         zucmotExtIOStruct->MbSlaveShmStruct.discrete_input[MB_D_INPT_GEN_0 + func_do.index - zucmotConfig->cabUsrPhysNumDIO] = do_stat;
    //     }
    //     else if (func_do.index < zucmotConfig->cabUsrPhysNumDIO + ZUCMOT_MAX_MODBUS_DIO + ZUCMOT_MAX_PNDev_DIO)
    //     {
    //         int pfnindex = func_do.index - zucmotConfig->cabUsrPhysNumDIO - ZUCMOT_MAX_MODBUS_DIO;
    //         zucmotPNDevStruct->pnio.DO[pfnindex] = do_stat;
    //     }
    // }
    // else if (func_do.type == 1 && func_do.index >= 0 && func_do.index < 2)
    // {
    //     tio_set_dout(func_do.index, do_stat);
    // }
    // else if (func_do.type == 2 && func_do.index >= 0 && func_do.index < MAX_EXTIO_DO_NUM)
    // {
    //     zucmotExtIOStruct->extIOShmStruct.extIOCtrl.extDO[func_do.index] = do_stat;
    // }
    // else if (func_do.type == 4 && func_do.index >= 0 && func_do.index < ZUCMOT_MAX_MODBUS_DIO)
    // {  // Modbus slave IO
    //     // zucmot_MB_Slave_DoutWrite(func_do.index, do_stat);
    //     write_DIO_i((IODevType)func_do.type, func_do.index, do_stat);
    // }
    // else if (func_do.type == 5 && func_do.index >= 0 && func_do.index < ZUCMOT_MAX_PNDev_DIO)
    // {  // profinet slave IO
    //     // zucmot_PN_DEV_DoutWrite(func_do.index, do_stat);
    //     write_DIO_i((IODevType)func_do.type, func_do.index, do_stat);
    // }
    // else if (func_do.type == 6)
    // {  // ethernet/ip slave IO
    //     if (zucmotStatus->eip_mode == 1 && func_do.index >= 0 && func_do.index < zucmotStatus->eip_do_num)
    //     {
    //         // zucmot_EIP_ADPT_DoutWrite(func_do.index, do_stat);
    //         write_DIO_i((IODevType)func_do.type, func_do.index, do_stat);
    //     }
    //     else if (zucmotStatus->eip_mode == 0 && func_do.index >= 0 && func_do.index < ZUCMOT_MAX_EIP_DIO)
    //     {
    //         // zucmot_EIP_ADPT_DoutWrite(func_do.index, do_stat);
    //         write_DIO_i((IODevType)func_do.type, func_do.index, do_stat);
    //     }
    // }
}